import rclpy
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from cv_bridge import CvBridge
import cv2
import mediapipe as mp
from unitree_api.msg import Request
import json
import threading
import queue
import time

class HandGestureControl(Node):
    def __init__(self):
        super().__init__('hand_gesture_control')
        
        # 初始化MediaPipe手部识别
        self.mp_hands = mp.solutions.hands
        self.hands = self.mp_hands.Hands(
            max_num_hands=1,
            min_detection_confidence=0.7,
            min_tracking_confidence=0.5)
        self.mp_draw = mp.solutions.drawing_utils
        
        # 初始化CV桥
        self.bridge = CvBridge()
        
        # 图像处理队列
        self.image_queue = queue.Queue(maxsize=1)
        self.stop_event = threading.Event()
        
        # 订阅压缩图像话题
        self.subscription = None
        self.create_subscription_()
        
        # 创建发布器
        self.pub = self.create_publisher(Request, "/api/sport/request", 10)
        
        # 定义API ID
        self.HELLO_API_ID = 1016
        
        # 手势状态变量
        self.last_gesture = None
        self.subscription_active = True
        
        # 启动处理线程
        self.processing_thread = threading.Thread(target=self.process_images)
        self.processing_thread.start()
        
        self.get_logger().info("手势控制节点已初始化")

    def create_subscription_(self):
        """创建或重新创建图像订阅"""
        if self.subscription is None or self.subscription_active is False:
            self.subscription = self.create_subscription(
                CompressedImage,
                '/camera/image_raw',
                self.image_callback,
                10)
            self.subscription_active = True
            self.get_logger().info("已激活相机订阅")

    def destroy_subscription_(self):
        """销毁图像订阅"""
        if self.subscription is not None and self.subscription_active:
            self.destroy_subscription(self.subscription)
            self.subscription_active = False
            self.get_logger().info("已暂停相机订阅")

    def image_callback(self, msg):
        try:
            # 将压缩图像消息转换为OpenCV格式
            cv_image = self.bridge.compressed_imgmsg_to_cv2(msg, desired_encoding="bgr8")
            
            # 将图像放入队列供处理线程使用
            if not self.image_queue.full():
                self.image_queue.put(cv_image, block=False)
        except Exception as e:
            self.get_logger().error(f"图像回调函数出错: {str(e)}")

    def process_images(self):
        while not self.stop_event.is_set() and rclpy.ok():
            try:
                # 从队列获取图像（带超时以避免永久阻塞）
                cv_image = self.image_queue.get(timeout=0.1)
                
                # 处理图像并检测手势
                results = self.process_image(cv_image)
                
                # 显示处理后的图像（可选）
                cv2.imshow('手势识别', cv_image)
                cv2.waitKey(1)
                
                # 根据手势结果发布控制命令
                self.handle_gesture(results)
                
            except queue.Empty:
                # 队列为空，继续尝试
                continue
            except Exception as e:
                self.get_logger().error(f"图像处理出错: {str(e)}")

    def process_image(self, image):
        # 转换颜色空间
        image_rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
        
        # 处理图像
        results = self.hands.process(image_rgb)
        
        # 如果检测到手部，绘制关键点
        if results.multi_hand_landmarks:
            for hand_landmarks in results.multi_hand_landmarks:
                self.mp_draw.draw_landmarks(
                    image, hand_landmarks, self.mp_hands.HAND_CONNECTIONS)
        
        return results

    def handle_gesture(self, results):
        if not results.multi_hand_landmarks:
            # 没有检测到手部
            if self.last_gesture != "stop":
                self.send_stop_command()
                self.last_gesture = "stop"
            return
        
        # 检查是否是两个手指的手势
        if self.is_two_fingers_gesture(results):
            if self.last_gesture != "hello":
                self.send_hello_command()
                self.last_gesture = "hello"
                # 检测到双指手势后暂停订阅15秒
                self.pause_subscription_for(15)
        else:
            if self.last_gesture != "stop":
                self.send_stop_command()
                self.last_gesture = "stop"

    def pause_subscription_for(self, seconds):
        """暂停订阅指定秒数"""
        if self.subscription_active:
            self.destroy_subscription_()
            
            # 启动定时器在指定时间后重新创建订阅
            timer_thread = threading.Thread(
                target=self.resume_subscription_after,
                args=(seconds,))
            timer_thread.start()

    def resume_subscription_after(self, seconds):
        """在指定秒数后恢复订阅"""
        time.sleep(seconds)
        self.create_subscription_()

    def is_two_fingers_gesture(self, results):
        """
        检测是否是两个手指的手势（例如"V"字手势）
        """
        for hand_landmarks in results.multi_hand_landmarks:
            landmarks = hand_landmarks.landmark
            
            # 获取关键点坐标
            thumb_tip = landmarks[4]
            index_tip = landmarks[8]
            middle_tip = landmarks[12]
            ring_tip = landmarks[16]
            pinky_tip = landmarks[20]
            
            # 判断手指是否伸直
            index_extended = index_tip.y < landmarks[6].y  # 指尖y坐标小于指节
            middle_extended = middle_tip.y < landmarks[10].y
            ring_bent = ring_tip.y > landmarks[14].y
            pinky_bent = pinky_tip.y > landmarks[18].y
            
            if index_extended and middle_extended and ring_bent and pinky_bent:
                return True
        
        return False

    def send_hello_command(self):
        self.get_logger().info("检测到双指手势 - 发送HELLO指令")
        
        req = Request()
        req.header.identity.api_id = self.HELLO_API_ID
        req.parameter = json.dumps({})  # 空参数
        
        self.pub.publish(req)

    def send_stop_command(self):
        self.get_logger().info("未检测到手势 - 发送STOP指令")
        
        req = Request()
        req.header.identity.api_id = 1002  # BALANCESTAND
        req.parameter = json.dumps({"x": 0.0, "y": 0.0, "z": 0.0})
        
        self.pub.publish(req)

    def destroy_node(self):
        # 停止处理线程
        self.stop_event.set()
        self.processing_thread.join()
        
        # 清理资源
        self.hands.close()
        cv2.destroyAllWindows()
        
        super().destroy_node()

def main(args=None):
    rclpy.init(args=args)
    node = HandGestureControl()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("接收到键盘中断信号，正在关闭节点...")
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()